#include <nds.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <nds/touch.h>
#include <nds/input.h>
#include "nds/dma.h"
#include "nds/ndstypes.h"
#include "nds/arm9/sassert.h"
#include "nds/arm9/video.h"
#include "nds/arm9/cache.h"
#include "nds/arm9/math.h"
#include "nds/arm9/trig_lut.h"
#include "nds/dynamicArray.h"

#include "std_ground_bin.h"
#include "pyrwall_bin.h"

unsigned int rtri;				// Angle For The Triangle ( NEW )
unsigned int rquad;			// Angle For The Quad ( NEW )
unsigned char random; //Random Number test.
unsigned char pyramids=25; //Number of pyramids.
int textureID;
int textureID2;
int pyrx[128]; //Pyramid x position.
int pyry[128]; //Pyramid y position.
int pyrz[128]; //Pyramid z position.
int pyrv[128]; //Pyramid rotation.
int pyrxsize[128]; //Pyramid x size.
int pyrysize[128]; //Pyramid y size.
int pyrzsize[128]; //Pyramid z size.
int pyrdistance[128]; //The distance from the player.
int y1;
int y2;
int x1;
int x2;
unsigned int worldsize=60;
//float test=atan2(128,224)* DEGREES_IN_CIRCLE / 360; 
//COOL!
bool collision=false; //Whether we're colliding or not.
int amount1;
int amount2;
float angle;
float heading;
float xpos;
float ypos;
float zpos;
float	yrot;				// Y Rotation
float lookupdown = 0.0f;
unsigned char i;
float gravity=-0.03f; //Gravity...Makes things fall if you didn't know.
bool jump=false; //When set to true, you are rising, when set to false, you are falling.
float yspeed=0.0f; //How fast we are rising and/or falling.
float xspeed=0.0f; //How fast we move in the x direction. Used in calculating the continuous momentum after jumping.
float zspeed=0.0f; //How fast we move in the z direction. Used in calculating the continuous momentum after jumping.
float turnspeed=0.0f; //How fast we turn. Used in calculating the continuous momentum after jumping.
float landy=0.0f; //What position we will "land" at. This can change based on what platforms are below us.
float jumpvelocity=1.5f; //How fast to jump up.

float sin(float angle)
{
	int32 s = sinLerp((short)(angle * DEGREES_IN_CIRCLE / 360));

	return f32tofloat(s);
}

float cos(float angle)
{
	int32 c = cosLerp((short)(angle * DEGREES_IN_CIRCLE / 360));

	return f32tofloat(c);
}

int main(void) {

	///Initialize Below\////////////////////////////////////
	consoleDemoInit();  //setup the sub screen for printing
	videoSetMode(MODE_0_3D);// Setup the Main screen for 3D
	glInit(); // initialize the geometry engine
	glEnable(GL_ANTIALIAS); //Enable Anti-Aliasing
	glEnable(GL_TEXTURE_2D); //enable textures
	glClearColor(0,0,0,31); // BG must be opaque for AA to work
	glClearPolyID(63); // BG must have a unique polygon ID for AA to work
	glClearDepth(0x7FFF);
	vramSetBankA(VRAM_A_TEXTURE);
	glGenTextures(1, &textureID);
	glBindTexture(GL_TEXTURE_2D, textureID);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, TEXTURE_SIZE_256 , TEXTURE_SIZE_256, 0, TEXGEN_TEXCOORD, (u8*)std_ground_bin);
	glGenTextures(1, &textureID2);
	glBindTexture(GL_TEXTURE_2D, textureID2);
	glTexImage2D(GL_TEXTURE_2D, 1, GL_RGB, TEXTURE_SIZE_128 , TEXTURE_SIZE_128, 0, TEXGEN_TEXCOORD, (u8*)pyrwall_bin);
	glViewport(0,0,255,191);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(70, 256.0 / 192.0, 0.1, 100);
	gluLookAt(	0.0, 0.0, 1.0,		//camera possition 
				0.0, 0.0, 0.0,		//look at
				0.0, 1.0, 0.0);		//up
	glPolyFmt(POLY_ALPHA(31) | POLY_CULL_NONE);
	glColor3f(1, 1, 1);
	///////////////////////////////////////////////////////

createpyrs();
	
	srand(time(NULL));
	iprintf("Random number is %d\n", random);
	iprintf("This is a 3 Dimensional Test");
	

	while(1) {
	debug();
	scanKeys();
        int held = keysHeld();
        int pressed = keysDown();
		/*
		if (keysHeld() & KEY_A)
		{
			lookupdown -= 1.0f;
		}
		if (keysHeld() & KEY_B)
		{
			lookupdown += 1.0f;
		}
			Just temporarily (Or permanently) disable these.*/
			
		if (keysDown() & KEY_A) {//iprintf("Key detected.");
		if (yspeed==0.0f && jump==false) {jump=true;
		yspeed=jumpvelocity;
		//iprintf("Jumping!");
		}
		} //=( I want to jump more!
		
		if (jump==true) {
		ypos+=yspeed;
		yspeed+=gravity;
		//iprintf("Gravity!");
		if (ypos<landy) {ypos=landy; jump=false; yspeed=0.0f;
		//iprintf("Landed! Ready to jump again!");
		}
		heading += turnspeed;
		yrot = heading;
		xpos += xspeed;
		zpos += zspeed;
		} else {
		//We can move if we're not jumping! But we have to follow any previous movement speeds.
		if (keysHeld() & KEY_LEFT)
		{
			turnspeed = 2.25f;
			heading += turnspeed;
			yrot = heading;
		} else {
		if (jump==false) {
		turnspeed=0.0f;}
		}
		if (keysHeld() & KEY_RIGHT)
		{
			turnspeed = -2.25f;
			heading += turnspeed;
			yrot = heading;
		} else {
		if (jump==false & !KEY_LEFT) {
		turnspeed=0.0f;}
		}
		if (keysHeld() & KEY_DOWN)
		{
			xspeed = (float)sin(heading) * 0.6f;
			zspeed = (float)cos(heading) * 0.6f;
			xpos += xspeed;
			zpos += zspeed;

			for (i=0;i<pyramids;i+=1) {
					pyrdistance[i]=pow(pyrx[i]-xpos,2)+pow(pyrz[i]-zpos,2);
					if (pyrdistance[i]<pow(pyrxsize[i],2)*1.5) {
					//We are roughly within collision range!
					//Begin depth collision analysis!

					//First calculate angle from player to the pyr.
					y1=pyrz[i];
					y2=zpos;
					x1=pyrx[i];
					x2=xpos;
					angle=atan2(y1-y2,x1-x2);

					//Now for the final calculation!
					if (pyrdistance[i]>pow((pyrxsize[i])/cos(angle),2)) {collision=true;
					printf("\n!!!COLLISION!!! - %d,%f \n",pyrdistance[i],pow((pyrxsize[i])/cos(angle),2));
					} else {
					printf("\n%f",(float)angle);
					}


					}}
			if (collision==true) {
			xpos -= xspeed;
			zpos -= zspeed;
			}
			/*
			if (walkbiasangle >= 359.0f)
			{
				walkbiasangle = 0.0f;
			}
			else
			{
				walkbiasangle+= 10;
			}*/
			
			//walkbias = (float)sin(walkbiasangle)/20.0f;
		} else {
		if (jump==false) {
		xspeed=0.0f;
		zspeed=0.0f;}
		}
		if (keysHeld() & KEY_UP)
		{
			xspeed = -((float)sin(heading) * 0.6f);
			zspeed = -((float)cos(heading) * 0.6f);
			xpos += xspeed;
			zpos += zspeed;
			for (i=0;i<pyramids;i+=1) {
					pyrdistance[i]=pow(pyrx[i]-xpos,2)+pow(pyrz[i]-zpos,2);
					if (pyrdistance[i]<pow(pyrxsize[i],2)*1.5) {
					//We are roughly within collision range!
					//Begin depth collision analysis!

					//First calculate angle from player to the pyr.
					y1=pyrz[i];
					y2=zpos;
					x1=pyrx[i];
					x2=xpos;
					angle=atan2(y1-y2,x1-x2);

					//Now for the final calculation!
					if (pyrdistance[i]>pow((pyrxsize[i])/cos(angle),2)) {collision=true;
					printf("\n!!!COLLISION!!! - %d,%f \n",pyrdistance[i],pow((pyrxsize[i])/cos(angle),2));
					} else {
					printf("\n%f",(float)angle);
					}


					}}
			if (collision==true) {
			xpos -= xspeed;
			zpos -= zspeed;
			}
			/*
			if (walkbiasangle <= 1.0f)
			{
				walkbiasangle = 359.0f;
			}
			else
			{
				walkbiasangle-= 10;
			}*/
			//walkbias = (float)sin(walkbiasangle)/20.0f;
		} else {
		if (jump==false & !KEY_DOWN) {
		xspeed=0.0f;
		zspeed=0.0f;}
		}
		}

		//Perform 3D actions here.

				//ds specific, several attributes can be set here	
		glPolyFmt(POLY_ALPHA(31) | POLY_CULL_NONE);
		
		glMatrixMode(GL_TEXTURE);
		glLoadIdentity();
		// Set the current matrix to be the model matrix
		glMatrixMode(GL_MODELVIEW);
		
		//Push our original Matrix onto the stack (save state)
		glPushMatrix();	

		DrawGLScene();
		
		// Pop our Matrix from the stack (restore state)
		glPopMatrix(1);

		//a handy little built in function to wait for a screen refresh
		swiWaitForVBlank();
		
		// flush to screen	
		glFlush(0);
		
	}

	return 0;
}

int DrawGLScene() {
	generateworld();											// Done Drawing The Pyramid

	rtri+=1;											// Increase The Rotation Variable For The Triangle ( NEW )
	rquad+=1;										// Decrease The Rotation Variable For The Quad ( NEW )
	return TRUE;										// Keep Going
										// Keep Going
}

//This will render the pyramids.

int generateworld() {

	float xtrans = -xpos;
	float ztrans = -zpos;
	float ytrans = /*-walkbias*/-0.25f-ypos;
	float sceneroty = 360.0f - yrot;
	
collision=false;	

for (i=0;i<pyramids;i+=1) {
/*
if (pyrdistance[i]<pow(pyrxsize[i]*1.5,2)) {
//We are roughly within collision range!
//Begin depth collision analysis!

//First calculate angle from player to the pyr.
y1=pyrz[i];
y2=zpos;
x1=pyrx[i];
x2=xpos;
angle=atan2(y1-y2,x1-x2) * DEGREES_IN_CIRCLE / 360;

//Now for the final calculation!
if (pyrdistance[i]<pow((pyrxsize[i])/cos(angle),2)) {collision=true;
printf("\n!!!COLLISION!!! - %d,%f \n",pyrdistance[i],pow((pyrxsize[i]*1.5)/cos(angle),2));
} else {
printf("\n%f",(float)angle);
}


}*/
glLoadIdentity();									// Reset The Current Modelview Matrix
	//glTranslatef(+posx,+posy,+posz);
	glRotatef(lookupdown,1.0f,0,0);
	glRotatef(sceneroty,0,1.0f,0);
	glTranslatef(xtrans+pyrx[i], ytrans+pyry[i], ztrans+pyrz[i]);						// Move Left 1.5 Units And Into The Screen 6.0
	glRotatef(pyrv[i],0,1,0);						// Rotate The Triangle On The Y axis ( NEW )
	glScalef(pyrxsize[i],pyrysize[i],pyrzsize[i]);
	glBindTexture(GL_TEXTURE_2D, textureID2);
	glBegin(GL_TRIANGLES);								// Start Drawing A Triangle
		glNormal(NORMAL_PACK(0,inttov10(0),0));
		//glColor3f(1,0,0);						// Red
		//glVertex3f( 0, 1, 0);					// Top Of Triangle (Front)\
		GFX_TEX_COORD = (TEXTURE_PACK(0, inttot16(128)));
		glVertex3v16(floattov16(0),	floattov16(1), floattov16(0) );
		//glColor3f(0,1,0);						// Green
		//glVertex3f(-1,0, 1);					// Left Of Triangle (Front)
		GFX_TEX_COORD = (TEXTURE_PACK(inttot16(128), inttot16(128)));
		glVertex3v16(floattov16(-1),	floattov16(0), floattov16(1) );
		//glColor3f(0,0,1);						// Blue
		//glVertex3f( 1,0, 1);					// Right Of Triangle (Front)
		GFX_TEX_COORD = (TEXTURE_PACK(inttot16(128), inttot16(0)));
		glVertex3v16(floattov16(1),	floattov16(0), floattov16(1) );
		glColor3f(1,0,0);						// Red
		glVertex3f( 0, 1, 0);					// Top Of Triangle (Right)
		glColor3f(0,0,1);						// Blue
		glVertex3f( 1,0, 1);					// Left Of Triangle (Right)
		glColor3f(0,1,0);						// Green
		glVertex3f( 1,0, -1);					// Right Of Triangle (Right)
		glColor3f(1,0,0);						// Red
		glVertex3f( 0, 1, 0);					// Top Of Triangle (Back)
		glColor3f(0,1,0);						// Green
		glVertex3f( 1,0, -1);					// Left Of Triangle (Back)
		glColor3f(0,0,1);						// Blue
		glVertex3f(-1,0, -1);					// Right Of Triangle (Back)
		glColor3f(1,0,0);						// Red
		glVertex3f( 0, 1, 0);					// Top Of Triangle (Left)
		glColor3f(0,0,1);						// Blue
		glVertex3f(-1,0,-1);					// Left Of Triangle (Left)
		glColor3f(0,1,0);						// Green
		glVertex3f(-1,0, 1);					// Right Of Triangle (Left)
		
	glEnd();
	
	}
	
		glLoadIdentity();									// Reset The Current Modelview Matrix
		
			glMaterialf(GL_AMBIENT, RGB15(16,16,16));
		glMaterialf(GL_DIFFUSE, RGB15(16,16,16));
		glMaterialf(GL_SPECULAR, BIT(15) | RGB15(8,8,8));
		glMaterialf(GL_EMISSION, RGB15(16,16,16));

		//ds uses a table for shinyness..this generates a half-ass one
		glMaterialShinyness();
	glRotatef(lookupdown,1.0f,0,0);
	glRotatef(sceneroty,0,1.0f,0);
	glTranslatef(xtrans,ytrans,ztrans);						// Move Right 1.5 Units And Into The Screen 7.0
	glScalef(worldsize,1,worldsize); //Scale the ground by a lot.
	glBindTexture(GL_TEXTURE_2D, textureID);
	glBegin(GL_QUADS);									// Draw A Quad
			glNormal(NORMAL_PACK(0,inttov10(0),0));

			GFX_TEX_COORD = (TEXTURE_PACK(0, inttot16(256)));
			glVertex3v16(floattov16(-1),	floattov16(-1), floattov16(-1) );
	
			GFX_TEX_COORD = (TEXTURE_PACK(inttot16(256),inttot16(256)));
			glVertex3v16(floattov16(1),	floattov16(-1), floattov16(-1) );
	
			GFX_TEX_COORD = (TEXTURE_PACK(inttot16(256), 0));
			glVertex3v16(floattov16(1),	floattov16(-1), floattov16(1) );

			GFX_TEX_COORD = (TEXTURE_PACK(0,0));
			glVertex3v16(floattov16(-1),	floattov16(-1), floattov16(1) );
			/*
		glColor3f(0,1,0);						// Set The Color To Green
		glVertex3f( 1,-1, 1);					// Top Right Of The Quad (Bottom)
		glVertex3f(-1,-1, 1);					// Top Left Of The Quad (Bottom)
		glVertex3f(-1,-1,-1);					// Bottom Left Of The Quad (Bottom)
		glVertex3f( 1,-1,-1);					// Bottom Right Of The Quad (Bottom)
		*/
	glEnd();											// Done Drawing The Quad
	
	}
	
int debug() {
for (i=0;i<pyramids;i+=1) {
//iprintf("\nDistance from Pyramid %d: %d",i,pyrdistance[i]);
}
//printf("\n%f",(float)test);
}
	
//World random generation will be in here.

int createpyrs() {
for (i=0;i<pyramids;i+=1) {
pyrx[i]=rand() % worldsize - rand() % worldsize;
pyrz[i]=rand() % worldsize - rand() % worldsize;
if (pyrx[i]<10 && pyrx[i]>-10) {pyrx[i]+=10;}
if (pyrz[i]<10 && pyrz[i]>-10) {pyrz[i]+=10;}
pyrxsize[i]=rand() % 8+1;
pyrysize[i]=rand() % 10+1;
pyrzsize[i]=pyrxsize[i];
pyry[i]-=1;
//We'll leave Y untouched right now. Don't want to mess with heights.
//pyrv[i]=rand() % 360;
pyrv[i]=0;
}
}

//We are done making pyramids. Next would be to generate them.